#include "ms618_algfrontse1tose5edge.h"

using namespace MeasurE618;
AlgFrontSE1toSE5Edge::AlgFrontSE1toSE5Edge(int i):AlgBase(i)
{

}

void AlgFrontSE1toSE5Edge::operator()(cv::Mat &srcImg,
                                      int id,
                                      std::map<string, float> &measRes,
                                      int fixtureId)
{
//    qDebug() << " ";
    //front SE12345-> 27,28,29,30,31
    std::vector<std::vector<cv::Point2f>>resptVec;
    resptVec.resize(7);
    omp_set_num_threads(7);
#pragma omp parallel sections
    {
         #pragma omp section
        {
            int i=27;
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-27,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj. NewKirschEdgeOuter(imgt,tparas,contours);

            ImageCoordinateTransformation(contours,rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[0].push_back(ptsOut);
            }
        }
    }

#pragma omp parallel sections
    {
        #pragma omp section
        {
            int i=28;
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-27,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj. LDmeasure(imgt,tparas,contours);

            ImageCoordinateTransformation(contours,rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[1].push_back(ptsOut);
            }
        }


    }
#pragma omp parallel sections
    {
         #pragma omp section
        {
            int i=28;
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-26,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj. LDmeasure(imgt,tparas,contours);

            ImageCoordinateTransformation(contours,rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[2].push_back(ptsOut);
            }
        }


    }

#pragma omp parallel sections
    {
        #pragma omp section
        {

            int i=29;
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-26,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj. NewKirschEdgeOuter(imgt,tparas,contours);

            ImageCoordinateTransformation(contours,rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[3].push_back(ptsOut);
            }
        }


    }
#pragma omp parallel sections
    {
        #pragma omp section
        {
            int i=30;
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-26,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj. LDmeasure(imgt,tparas,contours);

            ImageCoordinateTransformation(contours,rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[4].push_back(ptsOut);
            }
        }


    }

#pragma omp parallel sections
    {
        #pragma omp section
        {
            int i=31;
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-26,j));
            }
            std::vector<cv::Rect> rangVec;

            TemplateMatch(srcImg, m_dataAllVec[i], rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();

            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj. NewKirschEdgeOuter(imgt, tparas, contours);
            ImageCoordinateTransformation(contours, rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x), static_cast<float>(elem.y)), ptsOut;
//                CoordinateTransformation(datumBackVec[id], ptsIn, ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[5].push_back(ptsOut);
            }
        }
    }

#pragma omp parallel sections
    {
         #pragma omp section
        {
            int i=28;
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-22,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj.NewKirschEdgeOuter(imgt,tparas,contours);

            ImageCoordinateTransformation(contours,rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[6].push_back(ptsOut);
            }
        }
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";

    if(resptVec[0].size()==2){
        measRes.insert(std::make_pair(string("FAI44-SE1-Y1-distance_pixel"),resptVec[0][0].y));
        float fai44se1y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI44-SE1-Y1-distance")], resptVec[0][0].y);
        measRes.insert(std::make_pair(string("FAI44-SE1-Y1-distance_real"), fai44se1y));

        measRes.insert(std::make_pair(string("FAI44-SE1-Y2-distance_pixel"),resptVec[0][1].y));
        float fai44se1y2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI44-SE1-Y2-distance")], resptVec[0][1].y);
        measRes.insert(std::make_pair(string("FAI44-SE1-Y2-distance_real"), fai44se1y2));
    }

    if(resptVec[1].size()==3){
        float fai48=(resptVec[1][0].x+resptVec[1][1].x+resptVec[1][2].x)/3.0;
        measRes.insert(std::make_pair(string("FAI48-SE2-X-distance_pixel"), -fai48));
        float fai48se2x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI48-SE2-X-distance")], -fai48);
        measRes.insert(std::make_pair(string("FAI48-SE2-X-distance_real"), fai48se2x));
    }

    if(resptVec[2].size()==3){
        float fai8=(resptVec[2][0].x+resptVec[2][1].x+resptVec[2][2].x)/3.0;
        measRes.insert(std::make_pair(string("FAI8-SE2-X-distance_pixel"), -fai8));
        float fai8se2x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI8-SE2-X-distance")], -fai8);
        measRes.insert(std::make_pair(string("FAI8-SE2-X-distance_real"), fai8se2x));
    }

    if(resptVec[3].size()==2){
        measRes.insert(std::make_pair(string("FAI66-SE3-Y1-distance_pixel"),resptVec[3][0].y));
        float fai66se3y1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI66-SE3-Y1-distance")], resptVec[3][0].y);
        measRes.insert(std::make_pair(string("FAI66-SE3-Y1-distance_real"), fai66se3y1));

        measRes.insert(std::make_pair(string("FAI66-SE3-Y2-distance_pixel"),resptVec[3][1].y));
        float fai66se3y2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI66-SE3-Y2-distance")], resptVec[3][1].y);
        measRes.insert(std::make_pair(string("FAI66-SE3-Y2-distance_real"), fai66se3y2));
    }

    if(resptVec[4].size()==3){
        float fai72=(resptVec[4][0].x+resptVec[4][1].x+resptVec[4][2].x)/3.0;
        measRes.insert(std::make_pair(string("FAI72-SE4-X-distance_pixel"), -fai72));
        float fai72se4x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI72-SE4-X-distance")], -fai72);
        measRes.insert(std::make_pair(string("FAI72-SE4-X-distance_real"), fai72se4x));
    }

    if(resptVec[5].size()==3){
        float fai49=(resptVec[5][0].y+resptVec[5][1].y+resptVec[5][2].y)/3.0;
        measRes.insert(std::make_pair(string("FAI49-SE5-Y-distance_pixel"),fai49));
        float fai49se5y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI49-SE5-Y-distance")], fai49);
        measRes.insert(std::make_pair(string("FAI49-SE5-Y-distance_real"), fai49se5y));
    }

    if(resptVec[6].size()==3){
        float fai47=(resptVec[6][0].x+resptVec[6][1].x+resptVec[6][2].x)/3.0;
        measRes.insert(std::make_pair(string("FAI47-SE2-X-distance_pixel"), -fai47));
        float fai47se2x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI47-SE2-X-distance")], -fai47);
        measRes.insert(std::make_pair(string("FAI47-SE2-X-distance_real"), fai47se2x));
    }
}

